/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-09-27     huger       the first version
 */

#include "motor.h"

#include "app/app_io.h"

#define DBG_TAG "motor"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

MotorRun stMotorRun[3]=
{
    {
        enState : MOTOR_STOP,
        enDir : MOTOR_CW,
        ucRestFlag:0,       /*复位标识*/
        ucStop:1,           /*停止*/
        uiMoveSpeed:0,       /*移动速度*/
        uiAcceleratedSpeed:10,   /*加速度*/
        uiDecelerationSpeed:20,  /*减速度*/
        uiStartVelocity:0,  /*起始速度*/
        uiMaxVelocity: 40,    /*最大速度*/
        uiAngle:0,        /*角度*/
    }
};

MotorRampData srd[3]               = {{STOP,CW,0,0,0,0,0}};         // 加减速曲线变量
__IO int32_t  step_position[3]     = {0};           // 当前位置
__IO uint8_t  MotionStatus[3]      =  {0};           //是否在运动？0：停止，1：运动
__IO uint32_t time_channel[3] ={ TIM_CHANNEL_3,TIM_CHANNEL_2,TIM_CHANNEL_1 };
__IO uint32_t time_flag[3] = { TIM_FLAG_CC3,TIM_FLAG_CC2,TIM_FLAG_CC1 };


float_t screw_length[3]={20,20,20};//mm
float_t screw_lead[3]= {1,0.635,0.635};

void  motor_init(void)
{

    //m1
    rt_pin_mode(m1_nsleep_pin, PIN_MODE_OUTPUT);
    rt_pin_mode(m1_dir_pin, PIN_MODE_OUTPUT);
    rt_pin_mode(m1_enable_pin, PIN_MODE_OUTPUT);
    //m2
    rt_pin_mode(m2_nsleep_pin, PIN_MODE_OUTPUT);
    rt_pin_mode(m2_dir_pin, PIN_MODE_OUTPUT);
    rt_pin_mode(m2_enable_pin, PIN_MODE_OUTPUT);
    //m3
    rt_pin_mode(m3_nsleep_pin, PIN_MODE_OUTPUT);
    rt_pin_mode(m3_dir_pin, PIN_MODE_OUTPUT);
    rt_pin_mode(m3_enable_pin, PIN_MODE_OUTPUT);

    rt_pin_write(m1_nsleep_pin, 0);
    rt_pin_write(m2_nsleep_pin, 0);
    rt_pin_write(m3_nsleep_pin, 0);


    StepMotor_Output_Disable(0);
    StepMotor_Output_Disable(1);
    StepMotor_Output_Disable(2);


    MX_TIM3_Init();


 /* 使能定时器比较输出 */

    HAL_TIM_OC_Stop_IT(&htim3, TIM_CHANNEL_1);
    HAL_TIM_OC_Stop_IT(&htim3, TIM_CHANNEL_2);
    HAL_TIM_OC_Stop_IT(&htim3, TIM_CHANNEL_3);

//    __HAL_DBGMCU_FREEZE_TIM3();

    LOG_D("motor pin and timer init success");
}


#if 1

LinearInterpolation_TypeDef interpolation_para[3] ={
        {
                endpoint_pulse : 0,         //到达终点位置需要的脉冲数

                currentpoint_pluse : 0,     //当前位置 脉冲数

                deviation : 0,                //目标位置与当前位置偏差参数

                delay : 100,                 //周期比较值  控制运动速度

                min_pluse : -0x7fffffff,       //最小位置
                max_pluse : 0x7fffffff,       //最大位置

                self_check_status:0,       //自检标志

                dir : MOTOR_CW,
                limit_status :0,
        },
        {
                endpoint_pulse : 0,         //到达终点位置需要的脉冲数

                currentpoint_pluse : 0,     //当前位置 脉冲数

                deviation : 0,                //目标位置与当前位置偏差参数

                delay : 100,                 //周期比较值  控制运动速度

                min_pluse : -0x7fffffff,       //最小位置
                max_pluse : 0x7fffffff,       //最大位置

                self_check_status:0,       //自检标志

                dir : MOTOR_CW,
                limit_status :0,
        },
        {
                endpoint_pulse : 0,         //到达终点位置需要的脉冲数

                currentpoint_pluse : 0,     //当前位置 脉冲数

                deviation : 0,                //目标位置与当前位置偏差参数

                delay : 100,                 //周期比较值  控制运动速度

                min_pluse : -0x7fffffff,       //最小位置
                max_pluse : 0x7fffffff,       //最大位置

                self_check_status:0,       //自检标志

                dir : MOTOR_CW,
                limit_status :0,
        }
};

int8_t InterPolation_Move(int8_t motor_id,int32_t target_pluse,uint32_t time_t)
{
    //位置偏差计算
    interpolation_para[motor_id].deviation = target_pluse- interpolation_para[motor_id].currentpoint_pluse;

    //判断电机运动方向
    if(interpolation_para[motor_id].deviation < 0 )
    {
        if(target_pluse <= interpolation_para[motor_id].min_pluse)
        {
            interpolation_para[motor_id].limit_status=1;
            LOG_D("motor %d tar_pos minu overflow",motor_id);
        }
        else
        {
            interpolation_para[motor_id].endpoint_pulse = target_pluse;

            interpolation_para[motor_id].dir = MOTOR_CCW;
            StepMotor_Dir_Reversal(motor_id);
            interpolation_para[motor_id].limit_status=0;
        }

    }
    if(interpolation_para[motor_id].deviation > 0 )
    {
        if(target_pluse >= interpolation_para[motor_id].max_pluse)
        {
            interpolation_para[motor_id].limit_status=1;
            LOG_D("motor %d tar_pos max overflow",motor_id);
        }
        else
        {
            interpolation_para[motor_id].endpoint_pulse = target_pluse;
            interpolation_para[motor_id].dir = MOTOR_CW;
            StepMotor_Dir_Forward(motor_id);
            interpolation_para[motor_id].limit_status=0;
        }

    }
    //计算pwm输出周期
    if(!interpolation_para[motor_id].limit_status)
    {
        if(interpolation_para[motor_id].deviation!=0)
        {
            interpolation_para[motor_id].delay = time_t * 500 /  abs(interpolation_para[motor_id].deviation);

            if(srd[motor_id].run_state == STOP)
            {
                srd[motor_id].run_state = RUN;
                StepMotor_Output_Enable(motor_id);

                uint16_t count = __HAL_TIM_GET_COMPARE(&htim3,time_channel[motor_id]);
                __HAL_TIM_SET_COMPARE(&htim3,time_channel[motor_id],count+interpolation_para[motor_id].delay);
                HAL_TIM_OC_Start_IT(&htim3,time_channel[motor_id]);
            }
        }
        return 1;
    }else
    {
        return 0;
    }
}


void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{
    //定时器使用翻转模式，需要进入两次中断才输出一个完整脉冲
    __IO static uint8_t i[3]= {0};
    iomap map = {0};

    if ( htim ==  &htim3)
    {
        if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1 )
        {
            //设置比较值
            uint16_t count = __HAL_TIM_GET_COMPARE(&htim3,time_channel[2]);
            __HAL_TIM_SET_COMPARE(&htim3,time_channel[2], interpolation_para[2].delay + count);

            i[0]++;     // 定时器中断次数计数值
            if(i[0]==2) // 2次，说明已经输出一个完整脉冲
            {
              i[0]=0;   // 清零定时器中断次数计数值
              //位置累加
              if(interpolation_para[2].dir == MOTOR_CW) interpolation_para[2].currentpoint_pluse++;
              else{ interpolation_para[2].currentpoint_pluse--; }

              //判断位置是否到达目标位置以及限位
              if((interpolation_para[2].currentpoint_pluse == interpolation_para[2].endpoint_pulse) || \
                      ((interpolation_para[2].currentpoint_pluse <= interpolation_para[2].min_pluse)&&(interpolation_para[2].dir == MOTOR_CCW)) || \
                      ((interpolation_para[2].currentpoint_pluse >= interpolation_para[2].max_pluse)&&(interpolation_para[2].dir == MOTOR_CW)))
                  {
                      srd[2].run_state = STOP;


                      if((interpolation_para[2].currentpoint_pluse <= interpolation_para[2].min_pluse)||(interpolation_para[2].currentpoint_pluse >= interpolation_para[2].max_pluse))
                      {
                          interpolation_para[2].limit_status=1;
                          srd[0].run_state = STOP;
                          srd[1].run_state = STOP;
                      }
                  }
              //自检
              if(!interpolation_para[2].self_check_status)
              {
                  app_io_get_gpio_map(&map);
                  if(map.sensor_st[2])
                  {
                      interpolation_para[2].self_check_status=1;
                      srd[2].run_state = STOP;
                  }
              }
              //stop
              if(srd[2].run_state == STOP)
              {
                  // 关闭通道
                  HAL_TIM_OC_Stop_IT(&htim3,time_channel[2]);
                  __HAL_TIM_CLEAR_FLAG(&htim3, time_flag[2]);
                  StepMotor_Output_Disable(2);
                  StepMotor_Dir_Forward(2);
                  interpolation_para[2].endpoint_pulse = interpolation_para[2].currentpoint_pluse;
              }
            }
        }
        if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2 )
        {
            //设置比较值
            uint16_t count = __HAL_TIM_GET_COMPARE(&htim3,time_channel[1]);
            __HAL_TIM_SET_COMPARE(&htim3,time_channel[1],interpolation_para[1].delay + count);

            i[1]++;     // 定时器中断次数计数值
            if(i[1]==2) // 2次，说明已经输出一个完整脉冲
            {
              i[1]=0;   // 清零定时器中断次数计数值

              //位置累加
              if(interpolation_para[1].dir == MOTOR_CW) interpolation_para[1].currentpoint_pluse++;
              else{ interpolation_para[1].currentpoint_pluse--; }

              //判断位置是否到达目标位置以及限位
              if((interpolation_para[1].currentpoint_pluse == interpolation_para[1].endpoint_pulse) || \
                      ((interpolation_para[1].currentpoint_pluse <= interpolation_para[1].min_pluse)&&(interpolation_para[1].dir == MOTOR_CCW)) || \
                      ((interpolation_para[1].currentpoint_pluse >= interpolation_para[1].max_pluse)&&(interpolation_para[1].dir == MOTOR_CW)))
              {
                  srd[1].run_state = STOP;

                  if((interpolation_para[1].currentpoint_pluse <= interpolation_para[1].min_pluse)||(interpolation_para[1].currentpoint_pluse >= interpolation_para[1].max_pluse))
                  {
                      interpolation_para[1].limit_status=1;
                      srd[0].run_state = STOP;
                      srd[2].run_state = STOP;
                  }
              }
              //自检
              if(!interpolation_para[1].self_check_status)
              {
                  app_io_get_gpio_map(&map);
                  if(map.sensor_st[1]){
                      interpolation_para[1].self_check_status=1;
                      srd[1].run_state = STOP;
                  }

              }
              //stop
              if(srd[1].run_state == STOP)
              {
                  // 关闭通道
                  HAL_TIM_OC_Stop_IT(&htim3,time_channel[1]);
                  __HAL_TIM_CLEAR_FLAG(&htim3, time_flag[1]);
                  StepMotor_Output_Disable(1);
                  StepMotor_Dir_Forward(1);
                  interpolation_para[1].endpoint_pulse = interpolation_para[1].currentpoint_pluse;
              }
            }
        }
        if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_3 )
        {
            //设置比较值
            uint16_t count = __HAL_TIM_GET_COMPARE(&htim3,time_channel[0]);
            __HAL_TIM_SET_COMPARE(&htim3,time_channel[0],interpolation_para[0].delay + count);

            i[2]++;     // 定时器中断次数计数值
            if(i[2]==2) // 2次，说明已经输出一个完整脉冲
            {
              i[2]=0;   // 清零定时器中断次数计数值

              //位置累加
              if(interpolation_para[0].dir == MOTOR_CW) interpolation_para[0].currentpoint_pluse++;
              else{ interpolation_para[0].currentpoint_pluse--; }

              //判断位置是否到达目标位置以及限位
              if((interpolation_para[0].currentpoint_pluse == interpolation_para[0].endpoint_pulse) || \
                      ((interpolation_para[0].currentpoint_pluse <= interpolation_para[0].min_pluse)&&(interpolation_para[0].dir == MOTOR_CCW)) || \
                      ((interpolation_para[0].currentpoint_pluse >= interpolation_para[0].max_pluse)&&(interpolation_para[0].dir == MOTOR_CW)))
              {
                  srd[0].run_state = STOP;

                  if((interpolation_para[0].currentpoint_pluse <= interpolation_para[0].min_pluse)||(interpolation_para[0].currentpoint_pluse >= interpolation_para[0].max_pluse))
                  {
                      interpolation_para[0].limit_status=1;
                      srd[1].run_state = STOP;
                      srd[2].run_state = STOP;
                  }
              }
              //自检
              if(!interpolation_para[0].self_check_status)
              {
                  app_io_get_gpio_map(&map);
                  if(map.sensor_st[0]){
                      interpolation_para[0].self_check_status=1;
                      srd[0].run_state = STOP;
                  }

              }
              //stop
              if(srd[0].run_state == STOP)
              {
                  // 关闭通道
                  HAL_TIM_OC_Stop_IT(&htim3,time_channel[0]);
                  __HAL_TIM_CLEAR_FLAG(&htim3, time_flag[0]);
                  StepMotor_Output_Disable(0);
                  StepMotor_Dir_Forward(0);
                  interpolation_para[0].endpoint_pulse = interpolation_para[0].currentpoint_pluse;
              }
            }
        }
    }
}


uint8_t micro_adjust_platform_selfcheck(void)
{
    static int cnt=0;

    for(uint i=0;i<sizeof(interpolation_para)/sizeof(interpolation_para[0]);i++)
    {
        interpolation_para[i].endpoint_pulse = interpolation_para[i].min_pluse;
        StepMotor_Dir_Forward(i);

        srd[i].run_state = RUN;
        StepMotor_Output_Enable(i);

        uint16_t count = __HAL_TIM_GET_COMPARE(&htim3,time_channel[i]);
        __HAL_TIM_SET_COMPARE(&htim3,time_channel[i],count+interpolation_para[i].delay);
        HAL_TIM_OC_Start_IT(&htim3,time_channel[i]);
    }

    while((!interpolation_para[0].self_check_status)||(!interpolation_para[1].self_check_status)||(!interpolation_para[2].self_check_status))
    {
        rt_thread_mdelay(1000);
        if(cnt<4)cnt++;
        else{
            for(uint i=0;i<sizeof(interpolation_para)/sizeof(interpolation_para[0]);i++)
            {
                if(!interpolation_para[i].self_check_status)
                {
                    interpolation_para[i].endpoint_pulse = interpolation_para[i].max_pluse;
                    StepMotor_Dir_Reversal(i);

                    srd[i].run_state = RUN;
                    StepMotor_Output_Enable(i);

                    uint16_t count = __HAL_TIM_GET_COMPARE(&htim3,time_channel[i]);
                    __HAL_TIM_SET_COMPARE(&htim3,time_channel[i],count+interpolation_para[i].delay);
                    HAL_TIM_OC_Start_IT(&htim3,time_channel[i]);
                }
            }
            break;
        }
    }

    cnt=0;

    while((!interpolation_para[0].self_check_status)||(!interpolation_para[1].self_check_status)||(!interpolation_para[2].self_check_status))
    {
        rt_thread_mdelay(1000);
        if(cnt<8)cnt++;
        else
        {
            for(uint i=0;i<sizeof(interpolation_para)/sizeof(interpolation_para[0]);i++)
            {
                // 关闭通道
                srd[i].run_state = STOP;
                interpolation_para[i].currentpoint_pluse=0;
                interpolation_para[i].endpoint_pulse=0;
            }
            LOG_D("motor_self_check failed");
            return 0 ;
        }
    }



    for (uint var = 0; var < sizeof(interpolation_para)/sizeof(interpolation_para[0]); var++) {
        interpolation_para[var].currentpoint_pluse=0;
        interpolation_para[var].endpoint_pulse=0;
        interpolation_para[var].min_pluse= - ((float_t) screw_length[var]/screw_lead[var] * motor_subdivision/2.0);;
        interpolation_para[var].max_pluse =  ( (float_t)screw_length[var]/screw_lead[var] * motor_subdivision/2.0);
        LOG_D("motor %d:min_limit=%d,max_limit=%d",var,interpolation_para[var].min_pluse,interpolation_para[var].max_pluse);
    }

    LOG_D("motor_self_check success");
    return 1;
}


static int motor_pos_ctr(int argc, char *argv[])
{

    uint8_t channel = atoi(argv[1]);
    if(channel>2)return 0;
    int32_t tar_pos = atoi(argv[2])+ interpolation_para[channel].currentpoint_pluse;
    InterPolation_Move(channel,tar_pos,atoi(argv[3]));
}
MSH_CMD_EXPORT(motor_pos_ctr, num1:channle  num2:tar_pos);



#endif




#if 0
/**
  * 函数功能: 相对位置运动：运动给定的步数
  * 输入参数: step：移动的步数 (正数为顺时针，负数为逆时针).
              accel  加速度,实际值为accel*0.1*rad/sec^2
              decel  减速度,实际值为decel*0.1*rad/sec^2
              speed  最大速度,实际值为speed*0.1*rad/sec
  * 返 回 值: 无
  * 说    明: 以给定的步数移动步进电机，先加速到最大速度，然后在合适位置开始
  *           减速至停止，使得整个运动距离为指定的步数。如果加减速阶段很短并且
  *           速度很慢，那还没达到最大速度就要开始减速
  */
void StepMotor_AxisMoveRel( int16_t channel,int32_t step, uint32_t accel, uint32_t decel, uint32_t speed)
{

  __IO uint16_t tim_count;
  // 达到最大速度时的步数
  __IO uint32_t max_s_lim;
  // 必须要开始减速的步数（如果加速没有达到最大速度）
  __IO uint32_t accel_lim;

  if(MotionStatus[channel] != STOP)    // 只允许步进电机在停止的时候才继续
    return;
  if(step < 0) // 步数为负数
  {
    srd[channel].dir = CCW; // 逆时针方向旋转

    StepMotor_Dir_Reversal(channel);
    step =-step;   // 获取步数绝对值
  }
  else
  {
    srd[channel].dir = CW; // 顺时针方向旋转
    StepMotor_Dir_Forward(channel);
  }

  if(step == 1)    // 步数为1
  {
    srd[channel].accel_count = -1;   // 只移动一步
    srd[channel].run_state = DECEL;  // 减速状态.
    srd[channel].step_delay = 1000;  // 短延时
  }
  else if(step != 0)  // 如果目标运动步数不为0
  {
    // 我们的电机控制专题指导手册有详细的计算及推导过程

    // 设置最大速度极限, 计算得到min_delay用于定时器的计数器的值。
    // min_delay = (alpha / tt)/ w
    srd[channel].min_delay = (int32_t)(A_T_x10/speed);

    // 通过计算第一个(c0) 的步进延时来设定加速度，其中accel单位为0.1rad/sec^2
    // step_delay = 1/tt * sqrt(2*alpha/accel)
    // step_delay = ( tfreq*0.676/10 )*10 * sqrt( (2*alpha*100000) / (accel*10) )/100
    srd[channel].step_delay = (int32_t)((T1_FREQ_148 * sqrt(A_SQ / accel))/10);

    // 计算多少步之后达到最大速度的限制
    // max_s_lim = speed^2 / (2*alpha*accel)
    max_s_lim = (uint32_t)(speed*speed/(A_x200*accel/10));
    // 如果达到最大速度小于0.5步，我们将四舍五入为0
    // 但实际我们必须移动至少一步才能达到想要的速度
    if(max_s_lim == 0){
      max_s_lim = 1;
    }

    // 计算多少步之后我们必须开始减速
    // n1 = (n1+n2)decel / (accel + decel)
    accel_lim = (uint32_t)(step*decel/(accel+decel));
    // 我们必须加速至少1步才能才能开始减速.
    if(accel_lim == 0){
      accel_lim = 1;
    }

    // 使用限制条件我们可以计算出减速阶段步数
    if(accel_lim <= max_s_lim){
      srd[channel].decel_val = accel_lim - step;
    }
    else{
      srd[channel].decel_val = -(max_s_lim*accel/decel);
    }
    // 当只剩下一步我们必须减速
    if(srd[channel].decel_val == 0){
      srd[channel].decel_val = -1;
    }

    // 计算开始减速时的步数
    srd[channel].decel_start = step + srd[channel].decel_val;

    // 如果最大速度很慢，我们就不需要进行加速运动
    if(srd[channel].step_delay <= srd[channel].min_delay){
      srd[channel].step_delay = srd[channel].min_delay;
      srd[channel].run_state = RUN;
    }
    else{
      srd[channel].run_state = ACCEL;
    }
    // 复位加速度计数值
    srd[channel].accel_count = 0;
  }

  MotionStatus[channel] = 1; // 电机为运动状态
  switch(channel)
  {
      case 0:
          tim_count=__HAL_TIM_GET_COMPARE(&htim3,TIM_CHANNEL_1);
          __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,tim_count+srd[channel].step_delay); // 设置定时器比较值
          TIM_CCxChannelCmd(TIM3, TIM_CHANNEL_1, TIM_CCx_ENABLE);// 使能定时器通道
          break;
      case 1:
          tim_count=__HAL_TIM_GET_COMPARE(&htim3,TIM_CHANNEL_2);
          __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,tim_count+srd[channel].step_delay); // 设置定时器比较值
          TIM_CCxChannelCmd(TIM3, TIM_CHANNEL_2, TIM_CCx_ENABLE);// 使能定时器通道
          break;
      case 2:
          tim_count=__HAL_TIM_GET_COMPARE(&htim3,TIM_CHANNEL_3);
          __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,tim_count+srd[channel].step_delay); // 设置定时器比较值
          TIM_CCxChannelCmd(TIM3, TIM_CHANNEL_3, TIM_CCx_ENABLE);// 使能定时器通道
          break;
      default:break;
  }

  StepMotor_Output_Enable(channel);

}


/**
  * 函数功能: 定时器中断服务函数
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 实现加减速过程
  */
void TIM3_IRQHandler(void)//定时器中断处理
{
    __IO uint32_t tim_count=0;
    __IO uint32_t tmp = 0;
    // 保存新（下）一个延时周期
    uint16_t new_step_delay[3]={0};
    // 加速过程中最后一次延时（脉冲周期）.
    __IO static uint16_t last_accel_delay[3]={0};
    // 总移动步数计数器
    __IO static uint32_t step_count[3] = {0};
    // 记录new_step_delay中的余数，提高下一步计算的精度
    __IO static int32_t rest[3] = {0};
    //定时器使用翻转模式，需要进入两次中断才输出一个完整脉冲
    __IO static uint8_t i[3]= {0};

    if(__HAL_TIM_GET_IT_SOURCE(&htim3, TIM_IT_CC1) !=RESET)
    {
        // 清楚定时器中断
        __HAL_TIM_CLEAR_IT(&htim3, TIM_IT_CC1);

        // 设置比较值
        tim_count=__HAL_TIM_GET_COMPARE(&htim3,TIM_CHANNEL_1);
        tmp = tim_count+srd[0].step_delay;
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,tmp);

        i[0]++;     // 定时器中断次数计数值
        if(i[0]==2) // 2次，说明已经输出一个完整脉冲
        {
          i[0]=0;   // 清零定时器中断次数计数值
          switch(srd[0].run_state) // 加减速曲线阶段
          {
            case STOP:
              step_count[0] = 0;  // 清零步数计数器
              rest[0] = 0;        // 清零余值
              // 关闭通道
              TIM_CCxChannelCmd(TIM3, TIM_CHANNEL_1, TIM_CCx_DISABLE);
              __HAL_TIM_CLEAR_FLAG(&htim3, TIM_FLAG_CC1);
              StepMotor_Output_Enable(0);
              MotionStatus[0] = 0;  //  电机为停止状态
              break;

            case ACCEL:
              step_count[0]++;      // 步数加1
              if(srd[0].dir==CW)
              {
                step_position[0]++; // 绝对位置加1
              }
              else
              {
                step_position[0]--; // 绝对位置减1
              }
              srd[0].accel_count++; // 加速计数值加1
              new_step_delay[0] = srd[0].step_delay - (((2 *srd[0].step_delay) + rest[0])/(4 * srd[0].accel_count + 1));//计算新(下)一步脉冲周期(时间间隔)
              rest[0] = ((2 * srd[0].step_delay)+rest[0])%(4 * srd[0].accel_count + 1);// 计算余数，下次计算补上余数，减少误差
              if(step_count[0] >= srd[0].decel_start)// 检查是够应该开始减速
              {
                srd[0].accel_count = srd[0].decel_val; // 加速计数值为减速阶段计数值的初始值
                srd[0].run_state = DECEL;           // 下个脉冲进入减速阶段
              }
              else if(new_step_delay[0] <= srd[0].min_delay) // 检查是否到达期望的最大速度
              {
                last_accel_delay[0] = new_step_delay[0]; // 保存加速过程中最后一次延时（脉冲周期）
                new_step_delay[0] = srd[0].min_delay;    // 使用min_delay（对应最大速度speed）
                rest[0] = 0;                          // 清零余值
                srd[0].run_state = RUN;               // 设置为匀速运行状态
              }
              break;

            case RUN:
              step_count[0]++;  // 步数加1
              if(srd[0].dir==CW)
              {
                step_position[0]++; // 绝对位置加1
              }
              else
              {
                step_position[0]--; // 绝对位置减1
              }
              new_step_delay[0] = srd[0].min_delay;     // 使用min_delay（对应最大速度speed）
              if(step_count[0] >= srd[0].decel_start)   // 需要开始减速
              {
                srd[0].accel_count = srd[0].decel_val;  // 减速步数做为加速计数值
                new_step_delay[0] = last_accel_delay[0];// 加阶段最后的延时做为减速阶段的起始延时(脉冲周期)
                srd[0].run_state = DECEL;            // 状态改变为减速
              }
              break;

            case DECEL:
              step_count[0]++;  // 步数加1
              if(srd[0].dir==CW)
              {
                step_position[0]++; // 绝对位置加1
              }
              else
              {
                step_position[0]--; // 绝对位置减1
              }
              srd[0].accel_count++;
              new_step_delay[0] = srd[0].step_delay - (((2 * srd[0].step_delay) + rest[0])/(4 * srd[0].accel_count + 1)); //计算新(下)一步脉冲周期(时间间隔)
              rest[0] = ((2 * srd[0].step_delay)+rest[0])%(4 * srd[0].accel_count + 1);// 计算余数，下次计算补上余数，减少误差

              //检查是否为最后一步
              if(srd[0].accel_count >= 0)
              {
                  srd[0].run_state = STOP;
              }
              break;
          }
          srd[0].step_delay = new_step_delay[0]; // 为下个(新的)延时(脉冲周期)赋值
        }
    }
    if(__HAL_TIM_GET_IT_SOURCE(&htim3, TIM_IT_CC2) !=RESET)
    {
        // 清楚定时器中断
        __HAL_TIM_CLEAR_IT(&htim3, TIM_IT_CC2);

        // 设置比较值
        tim_count=__HAL_TIM_GET_COMPARE(&htim3,TIM_CHANNEL_2);
        tmp = tim_count+srd[1].step_delay;
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,tmp);

        i[1]++;     // 定时器中断次数计数值
        if(i[1]==2) // 2次，说明已经输出一个完整脉冲
        {
          i[1]=0;   // 清零定时器中断次数计数值
          switch(srd[1].run_state) // 加减速曲线阶段
          {
            case STOP:
              step_count[1] = 0;  // 清零步数计数器
              rest[1] = 0;        // 清零余值
              // 关闭通道
              TIM_CCxChannelCmd(TIM3, TIM_CHANNEL_2, TIM_CCx_DISABLE);
              __HAL_TIM_CLEAR_FLAG(&htim3, TIM_FLAG_CC2);
              StepMotor_Output_Enable(1);
              MotionStatus[1] = 0;  //  电机为停止状态
              break;

            case ACCEL:
              step_count[1]++;      // 步数加1
              if(srd[1].dir==CW)
              {
                step_position[1]++; // 绝对位置加1
              }
              else
              {
                step_position[1]--; // 绝对位置减1
              }
              srd[1].accel_count++; // 加速计数值加1
              new_step_delay[1] = srd[1].step_delay - (((2 *srd[1].step_delay) + rest[1])/(4 * srd[1].accel_count + 1));//计算新(下)一步脉冲周期(时间间隔)
              rest[1] = ((2 * srd[1].step_delay)+rest[1])%(4 * srd[1].accel_count + 1);// 计算余数，下次计算补上余数，减少误差
              if(step_count[1] >= srd[1].decel_start)// 检查是够应该开始减速
              {
                srd[1].accel_count = srd[1].decel_val; // 加速计数值为减速阶段计数值的初始值
                srd[1].run_state = DECEL;           // 下个脉冲进入减速阶段
              }
              else if(new_step_delay[1] <= srd[1].min_delay) // 检查是否到达期望的最大速度
              {
                last_accel_delay[1] = new_step_delay[1]; // 保存加速过程中最后一次延时（脉冲周期）
                new_step_delay[1] = srd[1].min_delay;    // 使用min_delay（对应最大速度speed）
                rest[1] = 0;                          // 清零余值
                srd[1].run_state = RUN;               // 设置为匀速运行状态
              }
              break;

            case RUN:
              step_count[1]++;  // 步数加1
              if(srd[1].dir==CW)
              {
                step_position[1]++; // 绝对位置加1
              }
              else
              {
                step_position[1]--; // 绝对位置减1
              }
              new_step_delay[1] = srd[1].min_delay;     // 使用min_delay（对应最大速度speed）
              if(step_count[1] >= srd[1].decel_start)   // 需要开始减速
              {
                srd[1].accel_count = srd[1].decel_val;  // 减速步数做为加速计数值
                new_step_delay[1] = last_accel_delay[1];// 加阶段最后的延时做为减速阶段的起始延时(脉冲周期)
                srd[1].run_state = DECEL;            // 状态改变为减速
              }
              break;

            case DECEL:
              step_count[1]++;  // 步数加1
              if(srd[1].dir==CW)
              {
                step_position[1]++; // 绝对位置加1
              }
              else
              {
                step_position[1]--; // 绝对位置减1
              }
              srd[1].accel_count++;
              new_step_delay[1] = srd[1].step_delay - (((2 * srd[1].step_delay) + rest[1])/(4 * srd[1].accel_count + 1)); //计算新(下)一步脉冲周期(时间间隔)
              rest[1] = ((2 * srd[1].step_delay)+rest[1])%(4 * srd[1].accel_count + 1);// 计算余数，下次计算补上余数，减少误差

              //检查是否为最后一步
              if(srd[1].accel_count >= 0)
              {
                  srd[1].run_state = STOP;
              }
              break;
          }
          srd[1].step_delay = new_step_delay[1]; // 为下个(新的)延时(脉冲周期)赋值
        }
    }
    if(__HAL_TIM_GET_IT_SOURCE(&htim3, TIM_IT_CC3) !=RESET)
    {
        // 清楚定时器中断
        __HAL_TIM_CLEAR_IT(&htim3, TIM_IT_CC3);

        // 设置比较值
        tim_count=__HAL_TIM_GET_COMPARE(&htim3,TIM_CHANNEL_3);
        tmp = tim_count+srd[2].step_delay;
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,tmp);

        i[2]++;     // 定时器中断次数计数值
        if(i[2]==2) // 2次，说明已经输出一个完整脉冲
        {
          i[2]=0;   // 清零定时器中断次数计数值
          switch(srd[2].run_state) // 加减速曲线阶段
          {
            case STOP:
              step_count[2] = 0;  // 清零步数计数器
              rest[2] = 0;        // 清零余值
              // 关闭通道
              TIM_CCxChannelCmd(TIM3, TIM_CHANNEL_3, TIM_CCx_DISABLE);
              __HAL_TIM_CLEAR_FLAG(&htim3, TIM_FLAG_CC3);
              StepMotor_Output_Enable(2);
              MotionStatus[2] = 0;  //  电机为停止状态
              break;

            case ACCEL:
              step_count[2]++;      // 步数加1
              if(srd[2].dir==CW)
              {
                step_position[2]++; // 绝对位置加1
              }
              else
              {
                step_position[2]--; // 绝对位置减1
              }
              srd[2].accel_count++; // 加速计数值加1
              new_step_delay[2] = srd[2].step_delay - (((2 *srd[2].step_delay) + rest[2])/(4 * srd[2].accel_count + 1));//计算新(下)一步脉冲周期(时间间隔)
              rest[2] = ((2 * srd[2].step_delay)+rest[2])%(4 * srd[2].accel_count + 1);// 计算余数，下次计算补上余数，减少误差
              if(step_count[2] >= srd[2].decel_start)// 检查是够应该开始减速
              {
                srd[2].accel_count = srd[2].decel_val; // 加速计数值为减速阶段计数值的初始值
                srd[2].run_state = DECEL;           // 下个脉冲进入减速阶段
              }
              else if(new_step_delay[2] <= srd[2].min_delay) // 检查是否到达期望的最大速度
              {
                last_accel_delay[2] = new_step_delay[2]; // 保存加速过程中最后一次延时（脉冲周期）
                new_step_delay[2] = srd[2].min_delay;    // 使用min_delay（对应最大速度speed）
                rest[2] = 0;                          // 清零余值
                srd[2].run_state = RUN;               // 设置为匀速运行状态
              }
              break;

            case RUN:
              step_count[2]++;  // 步数加1
              if(srd[2].dir==CW)
              {
                step_position[2]++; // 绝对位置加1
              }
              else
              {
                step_position[2]--; // 绝对位置减1
              }
              new_step_delay[2] = srd[2].min_delay;     // 使用min_delay（对应最大速度speed）
              if(step_count[2] >= srd[2].decel_start)   // 需要开始减速
              {
                srd[2].accel_count = srd[2].decel_val;  // 减速步数做为加速计数值
                new_step_delay[2] = last_accel_delay[2];// 加阶段最后的延时做为减速阶段的起始延时(脉冲周期)
                srd[2].run_state = DECEL;            // 状态改变为减速
              }
              break;

            case DECEL:
              step_count[2]++;  // 步数加1
              if(srd[2].dir==CW)
              {
                step_position[2]++; // 绝对位置加1
              }
              else
              {
                step_position[2]--; // 绝对位置减1
              }
              srd[2].accel_count++;
              new_step_delay[2] = srd[2].step_delay - (((2 * srd[2].step_delay) + rest[2])/(4 * srd[2].accel_count + 1)); //计算新(下)一步脉冲周期(时间间隔)
              rest[2] = ((2 * srd[2].step_delay)+rest[2])%(4 * srd[2].accel_count + 1);// 计算余数，下次计算补上余数，减少误差

              //检查是否为最后一步
              if(srd[2].accel_count >= 0)
              {
                  srd[2].run_state = STOP;
              }
              break;
          }
          srd[2].step_delay = new_step_delay[2]; // 为下个(新的)延时(脉冲周期)赋值
        }
    }
}

#endif

